/**
 * Snowflake MEGA sumo robot application to select a winning strategy
 * Copyright 2013 Maris BECS <maris.becs@gmail.com>
 */

#include <asf.h>
#include "keyboard.h"
#include "sensors.h"
#include "motors.h"
#include "strategies.h"
#include "strategies/strategy0.h"
#include "strategies/strategy1.h"
#include "strategies/strategy2.h"
//#include "strategies/strategy3.h"
//#include "strategies/strategy4.h"
//#include "strategies/strategy5.h"


void strategies_application(void)
{
	uint8_t strategy;
	struct keyboard_event input;

	// Clear screen
	gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);

	// Enable kill switch - press "8" on IR remote control to kill
	ioport_set_pin_low(START_SW);

	// Show constant label strategy selected
	gfx_mono_draw_string("STRATEGY:", 68, 0, &sysfont);
	
	// Read & Show strategy number(0-F) from the rotary switch at top right corner
	strategy = rotary_read();


	while (true) {
		bool enable_strategy_start = false;

		if ( LOW == ioport_get_pin_level(KILL_SW) ){
			// Kill switch active
			drive(0,0); // break motors
			delay_ms(100);
			stop_motors();  // free motors
		}

		// check key press
		keyboard_get_key_state(&input);
		if (input.type == KEYBOARD_RELEASE) {
			// BACK to exit FIGHT menu
			if (input.keycode == KEYBOARD_BACK) {
				// set IR module back to OFF state
				ioport_set_pin_high(START_SW);
				return;
			// ENTER to start the selected strategy
			} else if (input.keycode == KEYBOARD_ENTER) {
				enable_strategy_start = true;
				delay_ms(300);	// just in case
			// DOWN to read the strategy number(0-F) from the rotary switch
			} else if (input.keycode == KEYBOARD_DOWN) {
				strategy = rotary_read();
			}
		}

		if (0 == strategy) {
			// simplest strategy attack straight full speed
			gfx_mono_draw_string("Attack       STRAIGHT", 0, 14, &sysfont);
			gfx_mono_draw_string("   full speed forward", 0, 24, &sysfont);;
			if (enable_strategy_start) {
				strategy0_app();
				gfx_mono_draw_string("  ROBOT STOPPED  ", 0, 0, &sysfont);
			}

		} else if (1 == strategy) {
			// first move is to retrieve backwards left or right to the line
			gfx_mono_draw_string("Retrieve backwards to", 0, 14, &sysfont);
			gfx_mono_draw_string("                 line", 0, 24, &sysfont);
			if (enable_strategy_start) {
				strategy1_app();
				gfx_mono_draw_string("  ROBOT STOPPED  ", 0, 0, &sysfont);
			}

		} else if (2 == strategy) {
			// first move is to retrieve backwards left or right to the line and wait 2 sec
			gfx_mono_draw_string("Retrieve backwards to", 0, 14, &sysfont);
			gfx_mono_draw_string("line & wait to attack", 0, 24, &sysfont);
			if (enable_strategy_start) {
				strategy2_app();
				gfx_mono_draw_string("  ROBOT STOPPED  ", 0, 0, &sysfont);
			}

		} else if (3 == strategy) {
			// turn right or left after the start
			gfx_mono_draw_string("Turn right or left   ", 0, 14, &sysfont);
			gfx_mono_draw_string("      after the start", 0, 24, &sysfont);
			if (enable_strategy_start) {
				//strategy3_app();
				gfx_mono_draw_string("  ROBOT STOPPED  ", 0, 0, &sysfont);
			}

		} else if (4 == strategy) {
			// first move is to retrieve backwards left or right to the line
			gfx_mono_draw_string("Retrieve backwards to", 0, 14, &sysfont);
			gfx_mono_draw_string("                 line", 0, 24, &sysfont);
			if (enable_strategy_start) {
				//strategy4_app();
				gfx_mono_draw_string("  ROBOT STOPPED  ", 0, 0, &sysfont);
			}

		} else if (5 == strategy) {
			// first move is to retrieve backwards left or right to the line and wait 2 sec
			gfx_mono_draw_string("Retrieve backwards to", 0, 14, &sysfont);
			gfx_mono_draw_string("line & wait to attack", 0, 24, &sysfont);
			if (enable_strategy_start) {
				//strategy5_app();
				gfx_mono_draw_string("  ROBOT STOPPED  ", 0, 0, &sysfont);
			}

		}


	}
}	